// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.UnitTests
{
    using System;
    using System.Collections.Generic;
    using System.Drawing;
    using System.IO;
    using System.Linq;
    using System.Reflection;

    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Navigation;
    using Test.Robotics.Navigation.Metrics.PathPlanningAndTracking;

    /// <summary>
    /// The Horizontal Depth Profile unit test class
    /// </summary>
    [TestClass]
    public class PathPlanningTest
    {
        /// <summary>
        /// Test 1 neighbors for a cell with 4 neighbors
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void Get1NeighborsTest4Neighbors()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(5, 5), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(5, 6)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(5, 4)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(4, 5)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(6, 5)));
            Assert.IsTrue(neighbors.Count == 4);
        }

        /// <summary>
        /// Test 1 neighbors for a cell with 3 neighbors
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void Get1NeighborsTest3Neighbors()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(0, 5), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(0, 6)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(0, 4)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(1, 5)));
            Assert.IsTrue(neighbors.Count == 3);

            neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(9, 5), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(9, 6)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(9, 4)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(8, 5)));
            Assert.IsTrue(neighbors.Count == 3);

            neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(5, 0), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(6, 0)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(4, 0)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(5, 1)));
            Assert.IsTrue(neighbors.Count == 3);

            neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(5, 9), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(6, 9)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(4, 9)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(5, 8)));
            Assert.IsTrue(neighbors.Count == 3);
        }

        /// <summary>
        /// Test 1 neighbors for a cell with 2 neighbors
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void Get1NeighborsTest2Neighbors()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(0, 0), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(1, 0)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(0, 1)));
            Assert.IsTrue(neighbors.Count == 2);

            neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(9, 0), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(8, 0)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(9, 1)));
            Assert.IsTrue(neighbors.Count == 2);

            neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(0, 9), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(0, 8)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(1, 9)));
            Assert.IsTrue(neighbors.Count == 2);

            neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(9, 9), 10, 10);

            Assert.IsTrue(neighbors.Contains(new Point2D<int>(8, 9)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(9, 8)));
            Assert.IsTrue(neighbors.Count == 2);
        }

        /// <summary>
        /// Test Get1Neighbors bad input
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [ExpectedException(typeof(ArgumentException))]
        [Priority(0)]
        public void Get1NeighborsTestInvalidXCoordinate()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(10, 9), 10, 10);
        }

        /// <summary>
        /// Test Get1Neighbors bad input
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [ExpectedException(typeof(ArgumentException))]
        [Priority(0)]
        public void Get1NeighborsTestInvalidYCoordinate()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1Neighbors(new Point2D<int>(10, 9), 10, 10);
        }

        /// <summary>
        /// Test 1 neighbors with diagonals for a cell with 8 neighbors
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void Get1NeighborsWithDiagonals8NeighborTest()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(5, 5), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(4, 4)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(6, 6)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(4, 6)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(6, 4)));
            Assert.IsTrue(neighbors.Count == 8);
        }

        /// <summary>
        /// Test 1 neighbors with diagonals for a cell with 5 neighbors
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void Get1NeighborsWithDiagonals5NeighborTest()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(0, 5), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(1, 6)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(1, 4)));
            Assert.IsTrue(neighbors.Count == 5);

            neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(9, 5), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(8, 6)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(8, 4)));
            Assert.IsTrue(neighbors.Count == 5);

            neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(5, 0), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(6, 1)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(4, 1)));
            Assert.IsTrue(neighbors.Count == 5);

            neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(5, 9), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(6, 8)));
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(4, 8)));
            Assert.IsTrue(neighbors.Count == 5);
        }

        /// <summary>
        /// Test 1 neighbors with diagonals for a cell with 3 neighbors
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void Get1NeighborsWithDiagonals3NeighborTest()
        {
            List<Point2D<int>> neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(0, 0), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(1, 1)));
            Assert.IsTrue(neighbors.Count == 3);

            neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(9, 0), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(8, 1)));
            Assert.IsTrue(neighbors.Count == 3);

            neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(0, 9), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(1, 8)));
            Assert.IsTrue(neighbors.Count == 3);

            neighbors = PathPlanningCommon.Get1NeighborsWithDiagonals(new Point2D<int>(9, 9), 10, 10);

            // only check diagonal members -- other tests check 1-neighbors
            Assert.IsTrue(neighbors.Contains(new Point2D<int>(8, 8)));
            Assert.IsTrue(neighbors.Count == 3);
        }

        /// <summary>
        /// Test PointToArrayIndex bad X value
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [ExpectedException(typeof(ArgumentException))]
        [Priority(0)]
        public void PointToArrayIndexBadXTest()
        {
            int index = PathPlanningCommon.PointToArrayIndex(new Point2D<int>(16, 5), 10, 15);
        }

        /// <summary>
        /// Test PointToArrayIndex bad Y value
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [ExpectedException(typeof(ArgumentException))]
        [Priority(0)]
        public void PointToArrayIndexBadYTest()
        {
            int index = PathPlanningCommon.PointToArrayIndex(new Point2D<int>(11, 11), 10, 15);
        }

        /// <summary>
        /// Test GetLowestPotentialPoint
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void GetLowestPotentialPointTest()
        {
            List<Point2D<int>> locations = new List<Point2D<int>>();
            locations.Add(new Point2D<int>(1, 1));
            locations.Add(new Point2D<int>(1, 2));
            locations.Add(new Point2D<int>(3, 1));
            locations.Add(new Point2D<int>(1, 5));
            locations.Add(new Point2D<int>(7, 7));

            int[] potentialMap = new int[100];
            potentialMap[PathPlanningCommon.PointToArrayIndex(locations[0], 10, 10)] = 3;
            potentialMap[PathPlanningCommon.PointToArrayIndex(locations[1], 10, 10)] = 2;
            potentialMap[PathPlanningCommon.PointToArrayIndex(locations[2], 10, 10)] = 1;
            potentialMap[PathPlanningCommon.PointToArrayIndex(locations[3], 10, 10)] = 4;
            potentialMap[PathPlanningCommon.PointToArrayIndex(locations[4], 10, 10)] = 2;

            Assert.IsTrue(PathPlanningCommon.GetLowestPotentialPoint(potentialMap, locations, 10, 10) == locations[2]);
        }

        /// <summary>
        /// Test GetSteepestDecentPath()
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void GetSteepestDescentPathTest()
        {
            int[] potentialMap =
                this.ReadCsvFileToIntArray("Test.Robotics.Navigation.TestData.50x50CenterGoal.csv");

            // shortest path from (0,0) to goal (24,24) on this map should be along diagonal
            // map should be 50,50
            List<Point2D<int>> path = PathPlanningCommon.GetSteepestDescentPath(potentialMap, new Point2D<int>(0, 0), 50, 50);

            Assert.IsTrue(path.Count == 25);

            for (int i = 0; i < 25; i++)
            {
                Assert.IsTrue(path[i].X == i && path[i].Y == i);
            }

            // shortest path from (49,24) should be straight line along X dimension
            path = PathPlanningCommon.GetSteepestDescentPath(potentialMap, new Point2D<int>(49, 24), 50, 50);

            Assert.IsTrue(path.Count == 26);

            for (int i = 0; i < 26; i++)
            {
                Assert.IsTrue(path[i].X == 49 - i && path[i].Y == 24);
            }
        }

        /// <summary>
        /// Ensure Initialize method does not throw
        /// </summary>
        /// <param name="planner">Path planner.</param>
        public void PlannerInitializeTest(IGridBasedPathPlanner planner)
        {
            // simply ensure that initialize does not throw an exception given valid inputs
            planner.Initialize(50, 50, 0, 0);
        }

        /// <summary>
        /// Ensure path planner throws when there is no path to goal
        /// </summary>
        /// <param name="planner">Path planner.</param>
        public void PlannerNoPathPossibleTest(IGridBasedPathPlanner planner)
        {
            // start with a 50x50 grid where border is open space and rest of cells are free
            bool[] occupancyMap = this.LoadOccupancyMap("Test.Robotics.Navigation.TestData.50x50ObstacleOutline.csv");

            planner.Initialize(50, 50, 0, 0);

            // add an obstacle down the center
            for (int i = 1; i < 49; i++)
            {
                occupancyMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(i, 24), 50, 50)] = true;
            }

            // this should return an empty path
            List<Point2D<int>> path = (List<Point2D<int>>)planner.GetPath(occupancyMap, new Point2D<int>(1, 1), new Point2D<int>(48, 48));
            Assert.AreEqual(0, path.Count);
        }

        /// <summary>
        /// Ensure path planner throws when goal is inside an obstacle
        /// </summary>
        /// <param name="planner">Path planner.</param>
        public void PlannerGoalInsideObstacleTest(IGridBasedPathPlanner planner)
        {
            // will use a 50x50 grid where border is open space and rest of cells are free
            bool[] occupancyMap = this.LoadOccupancyMap("Test.Robotics.Navigation.TestData.50x50ObstacleOutline.csv");

            planner.Initialize(50, 50, 0, 0);

            // this should return an empty path as (49,49) is an obstacle
            List<Point2D<int>> path = (List<Point2D<int>>)planner.GetPath(occupancyMap, new Point2D<int>(1, 1), new Point2D<int>(49, 49));
            Assert.AreEqual(0, path.Count);
        }

        /// <summary>
        /// Ensure Initialize method does not throw
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void PathPlannerInitializeTest()
        {
            this.PlannerInitializeTest(new PathPlanner());
        }

        /// <summary>
        /// Ensure path planner throws when there is no path to goal
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void PathPlannerNoPathPossibleTest()
        {
            this.PlannerNoPathPossibleTest(new PathPlanner());
        }

        /// <summary>
        /// Test path planner behaves as expected with obstacle growth
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void PathPlannerWithGrownObstaclesFunctionalTest()
        {
            // start with a 50x50 grid where border is open space and rest of cells are free
            bool[] occupancyMap = this.LoadOccupancyMap("Test.Robotics.Navigation.TestData.50x50ObstacleOutline.csv");

            var planner = new PathPlanner();
            planner.Initialize(50, 50, 0, 0);

            // add an obstacle down the center
            for (int i = 2; i < 48; i++)
            {
                occupancyMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(i, 24), 50, 50)] = true;
            }

            // open the very center grid squre
            occupancyMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(24, 24), 50, 50)] = false;

            // this should give valid path around obstacle
            List<Point2D<int>> path = (List<Point2D<int>>)planner.GetPath(occupancyMap, new Point2D<int>(24, 1), new Point2D<int>(24, 47));
            Assert.AreEqual(46, path.Count);
            var expected = 
                new Point2D<int>[]
                {
                    new Point2D<int>(26, 2),
                    new Point2D<int>(27, 3),
                    new Point2D<int>(27, 4),
                    new Point2D<int>(27, 5),
                    new Point2D<int>(27, 6),
                    new Point2D<int>(26, 7),
                    new Point2D<int>(26, 8),
                    new Point2D<int>(26, 9),
                    new Point2D<int>(26, 10),
                    new Point2D<int>(26, 11),
                    new Point2D<int>(26, 12),
                    new Point2D<int>(26, 13),
                    new Point2D<int>(26, 14),
                    new Point2D<int>(26, 15),
                    new Point2D<int>(26, 16),
                    new Point2D<int>(26, 17),
                    new Point2D<int>(26, 18),
                    new Point2D<int>(26, 19),
                    new Point2D<int>(26, 20),
                    new Point2D<int>(26, 21),
                    new Point2D<int>(26, 22),
                    new Point2D<int>(25, 23),
                    new Point2D<int>(25, 24),
                    new Point2D<int>(25, 25),
                    new Point2D<int>(25, 26),
                    new Point2D<int>(25, 27),
                    new Point2D<int>(25, 28),
                    new Point2D<int>(25, 29),
                    new Point2D<int>(25, 30),
                    new Point2D<int>(25, 31),
                    new Point2D<int>(25, 32),
                    new Point2D<int>(25, 33),
                    new Point2D<int>(25, 34),
                    new Point2D<int>(25, 35),
                    new Point2D<int>(25, 36),
                    new Point2D<int>(25, 37),
                    new Point2D<int>(25, 38),
                    new Point2D<int>(24, 39),
                    new Point2D<int>(24, 40),
                    new Point2D<int>(24, 41),
                    new Point2D<int>(24, 42),
                    new Point2D<int>(24, 43),
                    new Point2D<int>(24, 44),
                    new Point2D<int>(24, 45),
                    new Point2D<int>(24, 46),
                    new Point2D<int>(24, 47)
                };
            for (var i = 0; i < 46; i++)
            {
                Assert.AreEqual(expected[i], path[i]);
            }

            // reinitialize planner with robot radius of l
            planner.Initialize(50, 50, 4, 2, 1);

            // this should return an empty path
            path = (List<Point2D<int>>)planner.GetPath(occupancyMap, new Point2D<int>(23, 1), new Point2D<int>(23, 48));
            Assert.AreEqual(0, path.Count);
        }

        /// <summary>
        /// Ensure path planner throws when goal is inside an obstacle
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void PathPlannerGoalInsideObstacleTest()
        {
            this.PlannerGoalInsideObstacleTest(new PathPlanner());
        }

        /// <summary>
        /// Check simple obstacle grow cases
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void ObstacleGrowTest()
        {
            // will use a 50x50 grid where border is open space and rest of cells are free
            bool[] occupancyMap = this.LoadOccupancyMap("Test.Robotics.Navigation.TestData.50x50ObstacleOutline.csv");

            int height = 50;
            int width = 50;

            bool[] grownMap = PathPlanningCommon.GrowObstacles(occupancyMap, 1, width, height);

            // ensure that obstacles form a border 2 thick
            for (int x = 0; x < 2; x++)
            {
                for (int y = 0; y < width; y++)
                {
                    Assert.IsTrue(
                        grownMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(x, y), width, height)]);
                    Assert.IsTrue(
                        grownMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(49 - x, y), width, height)]);
                }
            }

            for (int y = 0; y < 2; y++)
            {
                for (int x = 0; x < height; x++)
                {
                    Assert.IsTrue(
                        grownMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(x, y), width, height)]);
                    Assert.IsTrue(
                        grownMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(x, 49 - y), width, height)]);
                }
            }

            // ensure rest of grid squares are free
            for (int y = 2; y < 48; y++)
            {
                for (int x = 2; x < 48; x++)
                {
                    Assert.IsFalse(
                        grownMap[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(x, y), width, height)]);
                }
            }

            // will use a 50x50 grid where border is open space and rest of cells are free
            occupancyMap = this.LoadOccupancyMap("Test.Robotics.Navigation.TestData.50x50ObstacleGrowTest.csv");
            bool[] resultMap = this.LoadOccupancyMap("Test.Robotics.Navigation.TestData.50x50ObstacleGrowTestResults.csv");

            grownMap = PathPlanningCommon.GrowObstacles(occupancyMap, 3, width, height);

            for (int i = 0; i < occupancyMap.Length; i++)
            {
                Assert.AreEqual(resultMap[i], grownMap[i]);
            }
        }

        /// <summary>
        /// Test that no obstacles is reported as such.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        [Ignore]
        public void ObstacleDetectionNoneTest()
        {
            Assert.IsFalse(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 1,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that obstacle within range in front is reported as obstructing.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void ObstacleDetectionLargeNearInFrontTest()
        {
            Assert.IsTrue(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 1,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .      #    ..                  #" +
                "#                 ....  # ...                    #" +
                "#                     ..*.                       #" +
                "#                       #                        #" +
                "#                       #                        #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that obstacle out of range in front is reported as non-obstructing.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        [Ignore]
        public void ObstacleDetectionLargeFarInFrontTest()
        {
            Assert.IsFalse(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 1,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                     #          #" +
                "#          obstactle way in front ->  #          #" +
                "#                                   ..*          #" +
                "#   .......                        .  #...       #" +
                "#          ....                   .   #   ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that obstacle treated in range once distance is bumped up.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void ObstacleDetectionLargeFarInFrontButInRangeTest()
        {
            Assert.IsTrue(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 1,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 30, // distance bumped up
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                     #          #" +
                "#          obstactle way in front ->  #          #" +
                "#                                   ..*          #" +
                "#   .......                        .  #...       #" +
                "#          ....                   .   #   ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that no obstacle behind the robot is reported as non-obstructing.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        [Ignore]
        public void ObstacleDetectionLargeBehindTest()
        {
            Assert.IsFalse(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 1,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#       #   <- obstacle behind                   #" +
                "#       #                           ...          #" +
                "#   ....*..                        .   ...       #" +
                "#       #  ....                   .       ..     #" +
                "#       #      .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that no obstacle near path with small radius is reported as non-obstructing.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        [Ignore]
        public void ObstacleDetectionNearPathSmallRadiusTest()
        {
            Assert.IsFalse(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 1,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                      #  <- far enough away     #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that no obstacle near path with large radius is reported as obstructing.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void ObstacleDetectionNearPathLargeRadiusTest()
        {
            Assert.IsTrue(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 2, // radius bumped up
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                      #                         #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that too small obstacles might be missed.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        [Ignore]
        public void ObstacleDetectionNearPathUndetectedTest()
        {
            Assert.IsFalse(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 5, // large radius
                minObstacleSizeInMapCells: 5, // too large
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                      #                         #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that too small obstacles might be missed unless min size is adjusted.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void ObstacleDetectionNearPathDetectedTest()
        {
            Assert.IsTrue(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 5, // still large radius, but...
                minObstacleSizeInMapCells: 1, // min size small enough
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                      #                         #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that obstacles below the threshold are not detected.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        [Ignore]
        public void ObstacleDetectionLowThresholdTest()
        {
            Assert.IsFalse(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 5,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                      %  <- 0.35                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test that obstacles below the threshold are not detected.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void ObstacleDetectionHighThresholdTest()
        {
            Assert.IsTrue(this.ObstructionScenareo(
                width: 50,
                height: 20,
                resolution: 0.1f,
                radiusInMapCells: 5,
                minObstacleSizeInMapCells: 1,
                maxObstacleDistanceInMapCells: 10,
                obstacleThreshold: 0.35,
                occupancy:
                "##################################################" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                   ...          #" +
                "#   .......                        .   ...       #" +
                "#          ....                   .       ..     #" +
                "#              .                 .               #" +
                "#              R.               .                #" +
                "#                .           ..                  #" +
                "#                 ....    ...                    #" +
                "#                     ....                       #" +
                "#                                                #" +
                "#                      @  <- 0.36                #" +
                "#                                                #" +
                "#                                                #" +
                "#                                                #" +
                "##################################################"));
        }

        /// <summary>
        /// Test uncontrolled drive speed limiting.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void UncontrolledDriveLimitingTests()
        {
            // no limiting if haven't moved since drive command
            this.TestUncontrolledDriveLimiting(0.0, new DriveDirection(0, 1, 0.3), 1);

            // begin limiting as target distance is achieved
            this.TestUncontrolledDriveLimiting(0.1, new DriveDirection(0, 1, 0.3), 0.67);

            // limit more severely as closing in on target distance
            this.TestUncontrolledDriveLimiting(0.2, new DriveDirection(0, 1, 0.3), 0.33);

            // stop entirely once target distance achieved
            this.TestUncontrolledDriveLimiting(0.3, new DriveDirection(0, 1, 0.3), 0);
        }

        /// <summary>
        /// Test that random number generator produces a deterministic set of values.
        /// </summary>
        /// <remarks>This algorithm is shared by native C++ with a similar unit test.</remarks>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void SingleRandomNumber()
        {
            Assert.AreEqual(705894, PathPlanningAndTrackingManifest.Rand(42));
            Assert.AreEqual(117649, PathPlanningAndTrackingManifest.Rand(7));
            Assert.AreEqual(20739838, PathPlanningAndTrackingManifest.Rand(1234));
        }

        /// <summary>
        /// Test that random number generator produces a deterministic sequence of values.
        /// </summary>
        /// <remarks>This algorithm is shared by native C++ with a similar unit test.</remarks>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void RandomNumberSequence()
        {
            Assert.IsTrue(ArraysAreEqual(
                new int[]
                {
                    705894, 1126542223, 1579310009, 565444343, 807934826,
                    421520601, 2095673201, 1100194760, 1139130650, 552121545,
                    229968128, 1751246343, 1933904666, 970724117, 526968160,
                    531304892, 404315618, 694332618, 222172928, 1733822410,
                    1147638727, 1813450982, 1582736250, 168218361, 1157513875,
                    281338952, 1852259217, 997713207, 1013554273, 966378307,
                    501383488, 26451988, 49447387, 2133545567, 1965890610,
                    1687573175, 1225826296, 1651931201, 1339106791, 739215777,
                    806666144, 573618697, 755349096, 1376419055, 781211901,
                    113402349, 1135284754, 338656883, 974568031, 707121348,
                    413993338, 139015486, 2118548913, 1172713531, 191403351,
                    2133100698, 931428268, 1506597293, 401021674, 1167590632,
                    2137669385, 407939385, 1469442471, 857669597, 942678115,
                    1604214886, 382400917, 1741140195, 1731083343, 209296245,
                    63775929, 287698850, 1368882553, 816757960, 535562096,
                    1088182895, 1119178413, 222323218, 2112262793, 748593394,
                    1649968832, 569825713, 1431176418, 1965210926, 1001542422,
                    946661368, 1978755000, 1003527558, 2098587415, 687265577,
                    1705499073, 1858683402, 1594808152, 1197212457, 1775476056,
                    1140798127, 660120073, 737546509, 668566279, 959010049
                },
                PathPlanningAndTrackingManifest.RandSeq(42).Take(100).ToArray()));
        }

        /// <summary>
        /// Test that filtering of repeated random waypoints works as expected.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void RepeatedWaypointFiltering()
        {
            Assert.IsTrue(ArraysAreEqual(
                new int[] { 1, 2, 3, 4, 5, 6, 7, 8, 9 },
                PathPlanningAndTrackingManifest.FilterRepeats(
                    new int[] { 1, 2, 2, 3, 4, 4, 4, 5, 6, 6, 7, 8, 9, 9, 9 }).ToArray()));
        }

        /// <summary>
        /// Compare test sets (also used by Pioneer) against expected.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Path Planning")]
        [Priority(0)]
        public void CementLoopLandmarkSets()
        {
            AssertLandmarkSetsAreEqual(
                1,
                new int[] { 7, 1, 2, 0, 6, 3, 5, 0, 5, 4, 2, 3, 7, 1, 0, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                2,
                new int[] { 6, 2, 3, 4, 5, 0, 5, 7, 3, 0, 3, 1, 4, 6, 7, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                3,
                new int[] { 5, 3, 5, 7, 0, 4, 3, 1, 0, 6, 1, 6, 7, 3, 1, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                4,
                new int[] { 4, 7, 1, 2, 0, 2, 6, 7, 1, 6, 3, 0, 4, 6, 4, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                5,
                new int[] { 3, 5, 0, 4, 1, 0, 1, 2, 5, 1, 3, 0, 2, 7, 5, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                6,
                new int[] { 2, 6, 2, 6, 7, 1, 0, 6, 3, 2, 1, 4, 2, 5, 6, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                7,
                new int[] { 1, 7, 4, 1, 0, 6, 1, 2, 6, 1, 6, 5, 4, 5, 7, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                8,
                new int[] { 0, 1, 6, 3, 4, 1, 0, 5, 7, 3, 4, 6, 0, 4, 5, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                9,
                new int[] { 7, 2, 7, 6, 1, 0, 4, 1, 5, 3, 1, 3, 2, 3, 5, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);

            AssertLandmarkSetsAreEqual(
                10,
                new int[] { 6, 3, 1, 0, 1, 2, 0, 2, 4, 3, 7, 0, 4, 6, 3, -2 },
                PathPlanningAndTrackingManifest.GenerateCementLoopLandmarkIds);
        }

        /// <summary>
        /// Check that a set of generated waypoints matches the expected set.
        /// </summary>
        /// <param name="seed">Initial random seed.</param>
        /// <param name="expectedLandmarkIds">Expected landmark IDs.</param>
        /// <param name="generator">Waypoint generator function being tested.</param>
        private static void AssertLandmarkSetsAreEqual(int seed, int[] expectedLandmarkIds, Func<int, int, int[]> generator)
        {
            Assert.IsTrue(ArraysAreEqual(
                expectedLandmarkIds,
                generator(expectedLandmarkIds.Length - 1, seed)));
        }

        /// <summary>
        /// Compare contents of two arrays.
        /// </summary>
        /// <param name="a">First array.</param>
        /// <param name="b">Second array.</param>
        /// <returns>Whether arrays are equal.</returns>
        private static bool ArraysAreEqual(int[] a, int[] b)
        {
            return a.Zip(b, (x, y) => x == y).All(_ => _);
        }

        /// <summary>
        /// Create a GlobalMap, path plan and robot location, from an ASCII representation.
        /// </summary>
        /// <param name="width">Map width.</param>
        /// <param name="height">Map height.</param>
        /// <param name="resolution">Map cell resolution in meters.</param>
        /// <param name="radiusInMapCells">Robot radius in map cells (may be padded to account for deviation, etc.).</param>
        /// <param name="minObstacleSizeInMapCells">Minimum obstacle size in map cells.</param>
        /// <param name="maxObstacleDistanceInMapCells">Maximum obstacle distance in map cells beyond which we don't consider.</param>
        /// <param name="obstacleThreshold">Threshold above which map cell is considered occupied.</param>
        /// <param name="occupancy">String representation of occupancy.</param>
        /// <returns>Whether the path is obstructed.</returns>
        private bool ObstructionScenareo(
            int width,
            int height,
            float resolution,
            int radiusInMapCells,
            int minObstacleSizeInMapCells,
            int maxObstacleDistanceInMapCells,
            double obstacleThreshold,
            string occupancy)
        {
            GlobalMap map = new GlobalMap(width, height, new Pose2D(), resolution);
            List<Point2D<int>> plan = new List<Point2D<int>>();
            int robotX = 0;
            int robotY = 0;

            for (var i = 0; i < occupancy.Length; i++)
            {
                var x = i % width;
                var y = i / width;
                var val = 0.0f;
                switch (occupancy[i])
                {
                    case '#':
                        val = 1;
                        break;
                    case '%':
                        val = 0.35f;
                        break;
                    case '@':
                        val = 0.36f;
                        break;
                    case '.':
                        plan.Add(new Point2D<int>(x, y));
                        break;
                    case '*':
                        plan.Add(new Point2D<int>(x, y));
                        val = 1;
                        break;
                    case 'R':
                        robotX = x;
                        robotY = y;
                        break;
                }

                if (val > 0)
                {
                    map[x, y] = val;
                }
            }

            return PathPlanningCommon.Obstructed(
                map,
                new Point2D<int>(robotX, robotY),
                plan.OrderBy(p => p.X), // assumes left-to-right plan
                radiusInMapCells,
                minObstacleSizeInMapCells,
                maxObstacleDistanceInMapCells);
        }

        /// <summary>
        /// Turns a csv file into a 2-dimensional int array.  Assumes 1st line indicates
        /// array dimensions.
        /// </summary>
        /// <param name="filename">A CSV file.</param>
        /// <returns>Int array with data from CSV file.</returns>
        private int[] ReadCsvFileToIntArray(string filename)
        {
            char[] delimiterChars = { ',' };
            int[] array;
            int width;
            int height;

            using (
                StreamReader sr = new StreamReader(Assembly.GetExecutingAssembly().GetManifestResourceStream(filename)))
            {
                string sizeLine = sr.ReadLine();
                string[] sizesArray = sizeLine.Split(delimiterChars);

                width = Convert.ToInt32(sizesArray[0]);
                height = Convert.ToInt32(sizesArray[1]);

                array = new int[width * height];

                int row = 0;
                while (!sr.EndOfStream)
                {
                    string line = sr.ReadLine();
                    string[] cols = line.Split(delimiterChars);

                    if (cols.Length != width)
                    {
                        throw new Exception("unexepected number of cols");
                    }

                    for (int col = 0; col < cols.Length; col++)
                    {
                        array[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(row, col), width, height)] =
                            Convert.ToInt32(cols[col]);
                    }

                    row++;
                }

                if (row != height)
                {
                    throw new Exception("unexpected number of rows");
                }
            }

            return array;
        }

        /// <summary>
        /// Turns a csv file into a 2-dimensional int double.  Assumes 1st line indicates
        /// array dimensions.
        /// </summary>
        /// <param name="filename">A CSV file.</param>
        /// <returns>Double array with data from CSV file.</returns>
        private double[] ReadCsvFileToDoubleArray(string filename)
        {
            char[] delimiterChars = { ',' };
            double[] array;
            int width;
            int height;

            using (
                StreamReader sr = new StreamReader(Assembly.GetExecutingAssembly().GetManifestResourceStream(filename)))
            {
                string sizeLine = sr.ReadLine();
                string[] sizesArray = sizeLine.Split(delimiterChars);

                width = Convert.ToInt32(sizesArray[0]);
                height = Convert.ToInt32(sizesArray[1]);

                array = new double[width * height];

                int row = 0;
                while (!sr.EndOfStream)
                {
                    string line = sr.ReadLine();
                    string[] cols = line.Split(delimiterChars);

                    if (cols.Length != width)
                    {
                        throw new Exception("unexepected number of cols");
                    }

                    for (int col = 0; col < cols.Length; col++)
                    {
                        array[PathPlanningCommon.PointToArrayIndex(new Point2D<int>(row, col), width, height)] =
                            Convert.ToDouble(cols[col]);
                    }

                    row++;
                }

                if (row != height)
                {
                    throw new Exception("unexpected number of rows");
                }
            }

            return array;
        }

        /// <summary>
        /// Load occupancy map from resource.
        /// </summary>
        /// <param name="path">Resource path.</param>
        /// <returns>Occupancy map.</returns>
        private bool[] LoadOccupancyMap(string path)
        {
            return (from v in this.ReadCsvFileToDoubleArray(path) select v > 0.5).ToArray();
        }

        /// <summary>
        /// Test simulation of forward speed limiting during uncontrolled drive.
        /// </summary>
        /// <param name="distance">Distance of uncontrolled traversal.</param>
        /// <param name="direction">Drive direction including distance.</param>
        /// <param name="expectedForward">Expected forward speed after limiting.</param>
        private void TestUncontrolledDriveLimiting(double distance, DriveDirection direction, double expectedForward)
        {
            DriveDirection output =
                PathPlanningCommon.LimitForwardSpeed(
                    direction,
                    PathPlanningCommon.SpeedFactorApproachingTarget(direction, distance));

            Assert.AreEqual<double>(expectedForward, Math.Round(output.Forward, 2));
            Assert.AreEqual<double>(direction.Heading, output.Heading);
            Assert.AreEqual<double>(direction.Distance, output.Distance);
        }
    }
}
